#include "msp_parser.h"
#include <iostream>
#include <algorithm>

// 构造函数
MSPParser::MSPParser() : state_(MSP::IDLE), expectedSize_(0), readIndex_(0) {}

// 析构函数
MSPParser::~MSPParser() {}

// 解析接收到的数据
void MSPParser::parse(const uint8_t *data, size_t size) {
  for (size_t i = 0; i < size; i++) {
    uint8_t c = data[i];

    // 解析MSP协议帧
    switch (state_) {
      case MSP::IDLE:
        // 寻找帧开始标记 $
        if (c == MSP::MSP_V1_FLAG) {
          state_ = MSP::HEADER_START;
        }
        break;

      case MSP::HEADER_START:
        // 下一个字符应该是 'M'
        if (c == MSP::MSP_V1_DIRECTION_FROM_FC) {
          state_ = MSP::HEADER_M;
        } else {
          state_ = MSP::IDLE;
        }
        break;

      case MSP::HEADER_M:
        // 下一个字符应该是 '>'
        if (c == MSP::MSP_V1_RESPONSE) {
          state_ = MSP::HEADER_ARROW;
        } else {
          state_ = MSP::IDLE;
        }
        break;

      case MSP::HEADER_ARROW:
        // 下一个字符是数据长度
        expectedSize_ = c;
        currentFrame_.size = c;
        state_ = MSP::HEADER_SIZE;
        break;

      case MSP::HEADER_SIZE:
        // 下一个字符是命令ID
        currentFrame_.cmd = c;
        currentFrame_.payload.clear();
        readIndex_ = 0;

        // 处理空负载情况
        if (expectedSize_ == 0) {
          currentFrame_.checksum = currentFrame_.size ^ currentFrame_.cmd;
          state_ = MSP::CHECKSUM;
        } else {
          state_ = MSP::PAYLOAD;
        }
        break;

      case MSP::PAYLOAD:
        // 收集负载数据
        currentFrame_.payload.push_back(c);
        readIndex_++;

        // 如果已经读取了全部负载
        if (readIndex_ >= expectedSize_) {
          // 计算校验和
          currentFrame_.calculateChecksum();
          state_ = MSP::CHECKSUM;
        }
        break;

      case MSP::CHECKSUM:
        // 验证校验和
        if (c == currentFrame_.checksum) {
          // 帧接收完成
          handleFrame(currentFrame_);
          state_ = MSP::COMMAND_RECEIVED;
        } else {
          std::cerr << "校验和错误: 期望 " << (int)currentFrame_.checksum
                    << " 实际 " << (int)c << std::endl;
        }

        // 重置为初始状态
        state_ = MSP::IDLE;
        break;

      case MSP::COMMAND_RECEIVED:
        // 不应该到达这里，安全起见重置
        state_ = MSP::IDLE;
        break;
    }
  }
}

// 解析接收到的数据（vector版本）
void MSPParser::parse(const std::vector<uint8_t> &data) {
  parse(data.data(), data.size());
}

// 创建命令帧
MSP::Frame MSPParser::createFrame(uint8_t command) {
  MSP::Frame frame;
  frame.cmd = command;
  frame.size = 0;
  frame.calculateChecksum();
  return frame;
}

// 创建带负载的命令帧
MSP::Frame MSPParser::createFrame(uint8_t command,
                                  const std::vector<uint8_t> &payload) {
  MSP::Frame frame;
  frame.cmd = command;
  frame.payload = payload;
  frame.size = static_cast<uint8_t>(payload.size());
  frame.calculateChecksum();
  return frame;
}

// 设置帧回调函数
void MSPParser::setFrameCallback(FrameCallback callback) {
  frameCallback_ = callback;
}

// 设置姿态回调函数
void MSPParser::setAttitudeCallback(AttitudeCallback callback) {
  attitudeCallback_ = callback;
}

// 设置原始IMU数据回调函数
void MSPParser::setRawImuCallback(RawImuCallback callback) {
  rawImuCallback_ = callback;
}

// 解析姿态数据
MSP::AttitudeData MSPParser::parseAttitude(const MSP::Frame &frame) const {
  MSP::AttitudeData attitude;

  // MSP_ATTITUDE 应该有 6 个字节的负载
  if (frame.cmd == MSP::MSP_ATTITUDE && frame.payload.size() >= 6) {
    // 姿态角格式：roll, pitch, yaw，每个16位有符号整数
    // roll, pitch 单位是 1/10 度
    // yaw 单位是度
    int16_t roll = read16(frame.payload, 0);
    int16_t pitch = read16(frame.payload, 2);
    int16_t yaw = read16(frame.payload, 4);

    // 转换为浮点数表示的度数
    attitude.roll = roll / 10.0f;
    attitude.pitch = pitch / 10.0f;
    attitude.yaw = yaw / 1.0f;
  } else {
    std::cerr << "无效的姿态数据帧" << std::endl;
  }

  return attitude;
}

// 解析原始IMU数据
MSP::RawImuData MSPParser::parseRawImu(const MSP::Frame &frame) const {
  MSP::RawImuData imu;

  // MSP_RAW_IMU 应该有 18 个字节的负载
  if (frame.cmd == MSP::MSP_RAW_IMU && frame.payload.size() >= 18) {
    // 格式：acc[3], gyro[3], mag[3]，每个16位有符号整数
    for (int i = 0; i < 3; i++) {
      imu.acc[i] = read16(frame.payload, i * 2);
      imu.gyro[i] = read16(frame.payload, 6 + i * 2);
      imu.mag[i] = read16(frame.payload, 12 + i * 2);
    }
  } else {
    std::cerr << "无效的原始IMU数据帧" << std::endl;
  }

  return imu;
}

// 重置解析器状态
void MSPParser::reset() {
  state_ = MSP::IDLE;
  expectedSize_ = 0;
  readIndex_ = 0;
  currentFrame_ = MSP::Frame();
}

// 处理完整的帧
void MSPParser::handleFrame(const MSP::Frame &frame) {
  // 调用通用帧回调
  if (frameCallback_) {
    frameCallback_(frame);
  }

  // 根据命令类型调用特定回调
  switch (frame.cmd) {
    case MSP::MSP_ATTITUDE:
      if (attitudeCallback_) {
        attitudeCallback_(parseAttitude(frame));
      }
      break;

    case MSP::MSP_RAW_IMU:
      if (rawImuCallback_) {
        rawImuCallback_(parseRawImu(frame));
      }
      break;
  }
}

// 从数据中提取有符号16位整数（小端序）
int16_t MSPParser::read16(const std::vector<uint8_t> &buffer,
                          size_t offset) const {
  if (offset + 1 >= buffer.size()) {
    return 0;
  }

  return static_cast<int16_t>(buffer[offset]) |
         (static_cast<int16_t>(buffer[offset + 1]) << 8);
}

// 从数据中提取无符号16位整数（小端序）
uint16_t MSPParser::readU16(const std::vector<uint8_t> &buffer,
                            size_t offset) const {
  if (offset + 1 >= buffer.size()) {
    return 0;
  }

  return static_cast<uint16_t>(buffer[offset]) |
         (static_cast<uint16_t>(buffer[offset + 1]) << 8);
}

// 从数据中提取有符号32位整数（小端序）
int32_t MSPParser::read32(const std::vector<uint8_t> &buffer,
                          size_t offset) const {
  if (offset + 3 >= buffer.size()) {
    return 0;
  }

  return static_cast<int32_t>(buffer[offset]) |
         (static_cast<int32_t>(buffer[offset + 1]) << 8) |
         (static_cast<int32_t>(buffer[offset + 2]) << 16) |
         (static_cast<int32_t>(buffer[offset + 3]) << 24);
}

// 将16位整数转换为字节（小端序）
void MSPParser::write16(std::vector<uint8_t> &buffer, int16_t value) {
  buffer.push_back(value & 0xFF);
  buffer.push_back((value >> 8) & 0xFF);
}

// 将32位整数转换为字节（小端序）
void MSPParser::write32(std::vector<uint8_t> &buffer, int32_t value) {
  buffer.push_back(value & 0xFF);
  buffer.push_back((value >> 8) & 0xFF);
  buffer.push_back((value >> 16) & 0xFF);
  buffer.push_back((value >> 24) & 0xFF);
}